#include "StdAfx.h"
#include "MeasureBox.h"
#include "Measure.h"


CMeasureBox::CMeasureBox(void)
  : name_(_T("box0"))
  , num_boxs_(1)
  , num_edges_(1)
  , pairing_(false)
{
  Init();
}

CMeasureBox::CMeasureBox(const tstring &boxName, int numEdges, bool pairing)
  : name_(boxName)
  , num_boxs_(1)
  , num_edges_(numEdges)
  , pairing_(pairing)
{
  Init();

  box_translations_.resize(1, static_cast<int>(roi_.half_height*2.4+0.5));
  box_translations_[0] = 0;

  Reserve();
}

CMeasureBox::~CMeasureBox(void)
{
}

void CMeasureBox::Reset(void)
{
  ok_ = false;
  for(int e=0; e<num_edges_; ++e) {
    edges_x_[e]    = 0;
    edges_y_[e]    = 0;
    amplitudes_[e] = 0;
    distances_[e]  = 0;
  }

  text_x_ = 0;
  text_y_ = 0;
}

tstring CMeasureBox::TransitionString(void) const
{
  return CMeasureApp::EdgeTransitionToString(transition_);
}

tstring CMeasureBox::SelectString(void) const
{
  return CMeasureApp::EdgeSelectToString(select_);
}

void CMeasureBox::Init(void)
{
  roi_.center_x     = 100;
  roi_.center_y     = 100;
  roi_.half_width   = 90;
  roi_.half_height  = 30;
  roi_.angle        = 270;

  sigma_            = 1;
  interpolation_    = 0;
  threshold_        = 30;
  transition_       = kTransitionAgnostic;
  select_           = kSelectAll;

  algorithm_            = 0;
  max_num_points_       = 30;
  clipping_end_points_  = 0;
  iterations_           = 5;
  clipping_factor_      = 1.0;

  fuzzy_            = false;
  set_type_         = 0;
  pair_width_       = 1;
  fuzzy_threshold_  = 700;
  uncertainty_      = 300;

  text_x_           = 0;
  text_y_           = 0;
}

void CMeasureBox::Reserve(void)
{
  ok_ = false;
  edges_x_.resize(num_edges_, 0);
  edges_y_.resize(num_edges_, 0);
  amplitudes_.resize(num_edges_, 0);
  distances_.resize(num_edges_, 0);
}

long CMeasureBox::Find(const CCoordSys& cs)
{
  angle_box_2 boxROI = roi_;
  cs.Inverse(boxROI);

  Hlong imageWidth  = 0;
  Hlong imageHeight = 0;
  Herror herr = Halcon::get_image_size(theImage, &imageWidth, &imageHeight);
  if (H_MSG_TRUE != herr) {
    return herr;
  }

  Halcon::HMeasure measureHandle;
  measureHandle.GenMeasureRectangle2(boxROI.center_y, boxROI.center_x,
    RADIAN(boxROI.angle), boxROI.half_width, boxROI.half_height,
    imageWidth, imageHeight, "bilinear");

  BOOST_AUTO(find_edges, &CMeasureBox::FindEdge);
  if (pairing_) {
    find_edges = &CMeasureBox::FindEdgePairs;
  }

  Halcon::set_color(hHalconWnd, "blue");
  Halcon::set_line_width(hHalconWnd, 1);
  Halcon::set_draw(hHalconWnd, "margin");

  Halcon::disp_rectangle2(hHalconWnd, boxROI.center_y, boxROI.center_x,
    RADIAN(boxROI.angle), boxROI.half_width, boxROI.half_height);

  Hlong tAscent=0, tDescent=0, tWidth=0, tHeight=0;
  Halcon::get_string_extents(hHalconWnd, name_.c_str(),
    &tAscent, &tDescent, &tWidth, &tHeight); 
  point_2 tp = GetTextPos(boxROI);
  text_x_ = (Hlong)tp.x;
  text_y_ = (Hlong)tp.y-(tAscent+tDescent+tHeight)/2;

  (this->*find_edges)(boxROI, measureHandle);
  return CErrorCode::kOk;
}

long CMeasureBox::FindEdge(const angle_box_2& iroi,
  const Halcon::HMeasure &measure_handle)
{
  Halcon::HTuple rowEdge;
  Halcon::HTuple colEdge;
  Halcon::HTuple amp;
  Halcon::HTuple dist;

  Herror herr = Halcon::measure_pos(theImage, measure_handle, sigma_, threshold_,
    TransitionHstring(), "all", &rowEdge, &colEdge, &amp, &dist);

  if (H_MSG_TRUE != herr) {
    return herr;
  }

  if (rowEdge.Num()>=num_edges_) {
    switch(select_) {
    case kSelectFirst:
      Halcon::tuple_first_n(rowEdge, num_edges_-1, &rowEdge);
      Halcon::tuple_first_n(colEdge, num_edges_-1, &colEdge);
      Halcon::tuple_first_n(amp, num_edges_-1, &amp);
      Halcon::tuple_first_n(dist, num_edges_-1, &dist);
      break;

    case kSelectLast:
      Halcon::tuple_last_n(rowEdge, rowEdge.Num()-num_edges_, &rowEdge);
      Halcon::tuple_last_n(colEdge, colEdge.Num()-num_edges_, &colEdge);
      Halcon::tuple_last_n(amp, amp.Num()-num_edges_, &amp);
      Halcon::tuple_last_n(dist, dist.Num()-num_edges_+1, &dist);
      break;

    case kSelectFirstLast:
      {
        Halcon::HTuple sIndex;
        sIndex.Append(0).Append(rowEdge.Num()-1);
        Halcon::tuple_select(rowEdge, sIndex, &rowEdge);
        Halcon::tuple_select(colEdge, sIndex, &colEdge);
        Halcon::tuple_select(amp, sIndex, &amp);
        Halcon::tuple_sum(dist, &dist);
      }
      break;

    default:
      break;
    }
  }

  Halcon::set_color(hHalconWnd, "red");
  Halcon::set_line_width(hHalconWnd, 1);
  Halcon::disp_cross(hHalconWnd, rowEdge, colEdge,
    theApp.config().cross_size(), 0);

  if (rowEdge.Num() != num_edges_) {
    return CErrorCode::kError;
  }

  ok_ = true;
  for (int e=0; e<num_edges_; ++e) {
    edges_x_[e]     = colEdge[e].D();
    edges_y_[e]     = rowEdge[e].D();
    amplitudes_[e]  = amp[e].D();
  }

  distances_[0] = 0;
  for (int e=1; e<num_edges_; ++e) {
    distances_[e] = dist[e-1].D();
  }

  return CErrorCode::kOk;
}

long CMeasureBox::FindEdgePairs(const angle_box_2& iroi,
  const Halcon::HMeasure &measure_handle)
{
  Halcon::HTuple rowEdgeFirst;
  Halcon::HTuple colEdgeFirst;
  Halcon::HTuple ampFirst;
  Halcon::HTuple rowEdgeSecond;
  Halcon::HTuple colEdgeSecond;
  Halcon::HTuple ampSecond;
  Halcon::HTuple intraDist;
  Halcon::HTuple interDist;
  Halcon::HTuple rowEdgeCenter;
  Halcon::HTuple colEdgeCenter;

  Herror herr = H_MSG_FALSE;

  if (fuzzy_) {
    Halcon::HTuple xValues, yValues, sizeFunction, fuzzyScore;
    xValues.Append(pair_width_-uncertainty_).Append(pair_width_).Append(pair_width_+uncertainty_);
    yValues.Append(0.0).Append(1.0).Append(0.0);

    Halcon::create_funct_1d_pairs(xValues, yValues, &sizeFunction);
    Halcon::set_fuzzy_measure(measure_handle, "size", sizeFunction);

    herr = Halcon::fuzzy_measure_pairs(theImage, measure_handle, sigma_, threshold_,
      fuzzy_threshold_/1000.0, TransitionHstring(), &rowEdgeFirst, &colEdgeFirst,
      &ampFirst, &rowEdgeSecond, &colEdgeSecond, &ampSecond, &rowEdgeCenter,
      &colEdgeCenter, &fuzzyScore, &intraDist, &interDist);
  } else {
    herr = Halcon::measure_pairs(theImage, measure_handle, sigma_, threshold_,
      TransitionHstring(), "all", &rowEdgeFirst, &colEdgeFirst,
      &ampFirst, &rowEdgeSecond, &colEdgeSecond, &ampSecond, &intraDist, &interDist);
  }

  if (H_MSG_TRUE != herr) {
    return herr;
  }

  if (rowEdgeFirst.Num() > num_edges_/2) {
    switch(select_) {
    case kSelectFirst:
      Halcon::tuple_first_n(rowEdgeFirst, num_edges_/2-1, &rowEdgeFirst);
      Halcon::tuple_first_n(colEdgeFirst, num_edges_/2-1, &colEdgeFirst);
      Halcon::tuple_first_n(ampFirst, num_edges_/2-1, &ampFirst);
      Halcon::tuple_first_n(rowEdgeSecond, num_edges_/2-1, &rowEdgeSecond);
      Halcon::tuple_first_n(colEdgeSecond, num_edges_/2-1, &colEdgeSecond);
      Halcon::tuple_first_n(ampSecond, num_edges_/2-1, &ampSecond);
      Halcon::tuple_first_n(intraDist, num_edges_/2-1, &intraDist);
      Halcon::tuple_first_n(interDist, num_edges_/2-2, &interDist);
      break;

    case kSelectLast:
      Halcon::tuple_last_n(rowEdgeFirst, rowEdgeFirst.Num()-num_edges_/2, &rowEdgeFirst);
      Halcon::tuple_last_n(colEdgeFirst, colEdgeFirst.Num()-num_edges_/2, &colEdgeFirst);
      Halcon::tuple_last_n(ampFirst, ampFirst.Num()-num_edges_/2, &ampFirst);
      Halcon::tuple_last_n(rowEdgeSecond, rowEdgeSecond.Num()-num_edges_/2, &rowEdgeSecond);
      Halcon::tuple_last_n(colEdgeSecond, colEdgeSecond.Num()-num_edges_/2, &colEdgeSecond);
      Halcon::tuple_last_n(ampSecond, ampSecond.Num()-num_edges_/2, &ampSecond);
      Halcon::tuple_last_n(intraDist, intraDist.Num()-num_edges_/2, &intraDist);
      Halcon::tuple_last_n(interDist, interDist.Num()-num_edges_/2+1, &interDist);
      break;

    default:
      break;
    }
  }

  Halcon::set_color(hHalconWnd, "red");
  Halcon::set_line_width(hHalconWnd, 1);
  Halcon::disp_cross(hHalconWnd, rowEdgeFirst, colEdgeFirst,
    theApp.config().cross_size(), 0);
  Halcon::disp_cross(hHalconWnd, rowEdgeSecond, colEdgeSecond,
    theApp.config().cross_size(), 0);

  if (rowEdgeFirst.Num() != num_edges_/2) {
    return CErrorCode::kError;
  }

  ok_ = true;
  distances_[0] = 0;

  for (int e=0; e<num_edges_/2; ++e) {
    edges_x_[e*2]       = colEdgeFirst[e].D();
    edges_y_[e*2]       = rowEdgeFirst[e].D();
    amplitudes_[e*2]    = ampFirst[e].D();
    edges_x_[2*e+1]     = colEdgeSecond[e].D();
    edges_y_[2*e+1]     = rowEdgeSecond[e].D();
    amplitudes_[2*e+1]  = ampSecond[e].D();
    distances_[2*e+1]    = intraDist[e].D();
  }

  for (int e=1; e<num_edges_/2; ++e) {
    distances_[2*e]    = interDist[e-1].D();
  }

  return CErrorCode::kOk;
}

const char* CMeasureBox::TransitionHstring(void) const
{
  switch (transition_) 
  {
  case kBlackToWhite:
    return "positive";

  case kWhiteToBlack:
    return "negative";

  case kTransitionAgnostic:
    return "all";

  case kBlackToWhiteStrongest:
    return pairing_?"positive_strongest":"positive";

  case kWhiteToBlackStrongest:
    return pairing_?"negative_strongest":"negative";

  case kTransitionAgnosticStrongest:
    return pairing_?"all_strongest":"all";

  default:
    return "all";
  }
}

const char* CMeasureBox::SelectHstring(void) const
{
  switch (select_) 
  {
  case kSelectAll:
    return "all";

  case kSelectFirst:
    return "first";

  case kSelectLast:
    return "last";

  default:
    return "all";
  }
}

double CMeasureBox::Distance(int iedge, int jedge) const
{
  double dist = 0;

  if (iedge>jedge) {
    std::swap(iedge, jedge);
  }

  for (int n=iedge+1; n<=jedge; ++n) {
    dist += distances_[n];
  }

  return dist;
}

point_2 CMeasureBox::GetTextPos(const angle_box_2& box)
{
  angle_box_2 nbox = box;
  nbox.angle = angle_math::normalize(box.angle);

  point_2 tl = nbox.top_left();
  point_2 tr = nbox.top_right();
  point_2 bl = nbox.bottom_left();
  point_2 br = nbox.bottom_right();
  point_2 cp = point_2(nbox.center_x, nbox.center_y);

  segment_2 st(tl, tr);
  segment_2 sl(tl, bl);
  segment_2 sb(bl, br);
  segment_2 sr(tr, br);
  segment_2 si(cp, point_2(1.0e6, cp.y));

  point_2 i0, i1;
  if (si.intersects(st, i0, i1)==kUnique) {
    return i0;
  }

  if (si.intersects(sl, i0, i1)==kUnique) {
    return i0;
  }

  if (si.intersects(sb, i0, i1)==kUnique) {
    return i0;
  }

  si.intersects(sr, i0, i1);
  return i0;
}

